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Abstract —  In  this  paper  we  consider  cooperative  manipulation 
problems  where  a  large  group  (swarm)  of  non-articulated  mobile 
robots  is  trying  to  cooperatively  control  the  velocity  of  some 
larger  rigid  body  by  exerting  forces  around  its  perimeter.  We 
consider  a  second-order  dynamic  model  for  the  object  but 
use  a  simplified  contact  model.  We  seek  solutions  that  require 
minimal  information  sharing  among  the  swarm  members.  We 
present  a  velocity  control  law  that  is  asymptotically  stable.  In 
the  case  of  a  constant  desired  velocity,  it  is  shown  that  no 
coordination  is  required  between  the  swarm  members.  For  more 
complex  trajectories  we  introduce  a  decentralized  feed-forward 
component  that  uses  an  online  consensus  estimate  of  the  swarm’s 
configuration.  The  results  are  illustrated  in  simulation. 

I.  Introduction 

Robot  swarms1  are  large  groups  of  small,  relatively  unso¬ 
phisticated,  robots  working  in  concert  to  achieve  objectives 
that  are  beyond  the  capability  of  a  single  robot.  One  example 
of  an  application  that  can  benefit  from  this  approach  is 
non-prehensile  cooperative  manipulation,  where  a  group  of 
non-articulated  mobile  robots  attempts  to  transport  a  larger 
object  in  the  plane,  by  applying  forces  to  its  perimeter.  The 
advantages  of  the  swarm  are:  (1)  its  ability  to  distribute  applied 
forces  over  a  large  area,  achieving  an  enveloping  grasp  on  large 
objects;  and  (2)  the  maximum  wrench  the  swarm  can  exert 
increases  linearly  as  the  number  of  swarm  members  increases. 
We  are  particularly  interested  in  marine  applications  involving 
autonomous  tugboats  such  as  towing  disabled  ships  (ex.  U.S.S. 
Cole),  transporting  components  of  large  offshore  structures 
(ex.  oil  platforms),  or  positioning  littoral  protection  equipment 
(ex.  hydrophone  arrays).  Figure  1  depicts  our  marine  test-bed. 
However,  most  of  our  work  can  be  extended  to  ground  robots. 

Behavior-based  approaches  to  non-articulated  cooperative 
manipulation  yield  interesting  results  but  lack  performance 
guarantees  [15],  [12],  [5],  [7],  On  the  other  hand,  so  called 
“caging”  approaches  such  as  [17],  [20],  and  [13],  design 
controllers  which  force  robots  to  surround  the  object.  Inter¬ 
robot  spacing  is  constrained  to  be  small  enough  that  it  is 
impossible  for  the  object  to  “escape”  -  meaning  that  one 
can  prove  as  the  robots  move,  so  must  the  object.  However, 
the  problem  is  reduced  to  a  positioning  problem;  limiting  its 
applicability  to  manipulation  problems  that  are  not  essentially 
kinematic,  such  as  marine  problems. 

1  esposito@usna.edu.  Supported  by  ONR  N0001405WRY20391 . 


Fig.  1.  Experimental  test  bed.  A  group  of  6  unmanned  tugboats  (0.5  meters 
long)  and  a  scale  model  flat  bottomed  barge  (2  meters  long).  The  tugs  have 
articulated  magnetic  attachment  devices  used  to  grab  the  barge. 


More  applicable  to  marine  problems  are  methods  that 
consider  the  full  second  order  dynamics  of  the  system  such 
as  [16],  [18],  [14],  and  [3],  However  these  approaches  all 
require  some  centralized  decision  making  or  global  knowledge 
of  all  swarm  member’s  actions.  We  seek  solutions  that  consider 
full  second  order  dynamics,  are  fully  distributed,  and  provide 
performance  and  stability  guarantees  such  as  the  flocking 
control  strategies  presented  in  [10],  [19],  [4],  Eventually  we 
hope  to  extend  the  work  non-trivial  contact  models. 

In  this  paper  we  assume  a  group  of  agents  has  already 
established  contact  with  an  object  to  be  manipulated.  Here, 
we  do  not  explicitly  consider  the  synthesis  of  the  grasp 
(see  our  previous  work  [2])  or  motion  control  of  the  swarm 
prior  to  establishing  contact  with  the  object.  We  describe  the 
system  and  communications  model  in  Sect.  II.  The  primary 
contribution  of  this  paper  is  a  novel  control  law  that  lets 
each  agent  compute  an  applied  force  such  that  the  swarm 
is  able  to  drive  the  velocity  of  the  object  to  some  desired 
value,  with  minimal  information  sharing.  In  the  case  of  a 
constant  desired  velocity  and  no  drag  we  show  that  absolutely 
no  communication  between  agents  is  required  (Sect.  III).  In 
the  case  where  the  desired  velocity  varies  with  time,  and 
drag  forces  are  present,  the  controller  (Sect.  IV)  utilizes  a 
decentralized  estimate  of  the  feed  forward  terms  based  on 
the  consensus  protocol  [9],  Simulation  results  are  provided.  In 
Sections  V  and  VI  we  discuss  the  implications  of  the  control 
laws. 
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II.  Model 


Consider  a  rigid  body  (see  Fig.  2)  with  pose  p  = 
[x,y,i/j}T  £  SE( 2)  defined  relative  to  a  global  reference 
frame.  A  body  fixed  reference  frame  is  attached  to  the  center 
of  mass.  Linear  velocity  v  and  angular  velocity  to  are  defined 
in  the  body  frame.  R(ip)  is  a  rotation  matrix  converting  body 
velocities  to  global  velocities.  N  agents  are  attached  to  the 
body  at  points  n, . . .  ,rjv  £  R  ,  where  rl  is  assumed  to  be 
time  invariant  in  the  body  frame.  Each  of  the  agents  can  apply 
an  input  force  F,  £  R2  written  in  body  frame  coordinates.  We 
assume  the  agents  are  rigidly  attached  to  the  object  and  can 
therefore  apply  a  force  of  any  magnitude,  in  any  direction  (i.e. 
the  grasp  is  not  friction  assisted).  The  system  dynamics  are: 


V  = 

R(i>)[v,  w]T 

(i) 

N 

v  — 

—LU  X  V  +  F^ag  ~\~  M  1  Fi 

(2) 

N 

UJ  = 

1~drag  H”  J  ^  ^  ^ i  X  Fi. 

(3) 

2  —  1 


Where  the  positive  scalars  M  and  J  are  the  effective  mass 
and  planar  moment  of  inertia  of  the  object  along  with  the 
attached  agents.  We  assume  the  products  of  inertia  are  zero 
(i.e.  left/right  and  fore/aft  symmetry).  Fdrag  and  rc/rQg  are  drag 
forces  and  torques  the  object  experiences.  The  appropriate 
model  for  these  is  application  dependent  (eg.  ground-based 
vs.  marine,  low  vs.  high-speed,  etc.) 

Note  that  under  these  assumptions,  in  general  there  must  be 
at  least  2  agents,  TV  >  1,  which  are  not  co-located,  n  ^  7*2, 
to  ensure  small  time  local  controllability.  Under  the  contact 
model  described  here,  this  is  equivalent  to  requiring  a  force 
closure  grasp  [8], 

For  some  tasks  the  agents  will  need  to  share  information  via 
some  type  of  ad  hoc  wireless  radio  or  optical  communication 
link.  The  communication  network  is  modeled  as  a  graph 
G  =  (V,E).  Each  vertex  in  the  graph,  i  £  V,  represents 
an  agent  and  each  edge,  £  E,  represents  a  wireless 
communication  link  between  agent  i  and  j.  We  assume  the 
set  of  links  in  the  network  (its  topology)  is  static,  message 
transfer  is  synchronous,  that  each  edge  permits  an  unlimited 
data  transfer  rate,  and  that  there  are  no  time  delays  or  noise 
in  transmission.  A  network  G  is  said  to  be  connected  if  the 
communication  graph  G  is  connected  (i.e.  if  for  any  node  pair 
i,j  there  exists  an  edge  path  of  arbitrary  length  between  them). 
Occasionally  we  discuss  the  network  neighbors  of  agent  i,  the 
set  of  all  nodes  a  single  hop  away,  defined  as 

M  =  {j  £  V\3 eij}.  (4) 

III.  Constant  Velocity  Controller 

Assume  the  desired  velocity  of  the  object  is  a  constant  in 
the  global  frame,  v^,  as  well  as  the  desired  angular  velocity 
and  that  Fdrag  =  [0,  0]T,  and  t*. ag  =  0.  In  this  section 


Global 

Frame 


Fig.  2.  Swarm  Manipulation  Scenario.  N  agents  (dark  circles)  are  attached  to 
the  object  (shaded  polygon).  The  fixed  position  of  the  ith  tug  boat  is  defined 
by  the  vector  r., .  It  may  apply  a  force  /■ ;  in  any  direction. 

we  show  that  if  each  agent  works  to  regulate  the  velocity  at 
point  Vi 

Vi  =  v  +  to  x  r, .  (5) 

to  the  desired  value  of  Vd  +  ood  x  r*  the  overall  motion  of 
the  body  converges  to  the  desired  velocity.  No  coordination 
between  the  agents  is  required. 

Proposition  3.1:  Assume  v and  tod  are  constants  in  the 
global  frame.  Note  that  since  the  system  is  planar  t o^  =  tod- 
Define  error  signals  ev  =  RT  (tp)v^  —  v  and  eu  =  Ud  —  u>.  If 
each  agent  applies  the  control  law: 

Ei  <  Fi  —  T  x  (6) 

then  ev,  ew  — >  0  as  t  — >  00. 

PROOF:  To  verify  this,  define  the  Lyapunov  Function 

w  =  \elMev  +  (7) 

The  derivative  is 

W  =  e^Mev  +  e^Je'uj  (8) 

N 

—  e^M{—uj  x  RTVd  +  to  x  v  —  M-1  ^  Fi} 

i= 1 
N 

x  Fi}. 

i= 1 

Substituting  the  control  law  Fi  from  (6)  and  noting  that 

e^(ew  x  n)  =  e£(r;  x  e„), 

and 

eZ(r.i  x  (ew  x  r»))  =  (e5'ew)(r.fri), 

the  derivative  of  W  simplifies  to 

N 

W  =  -cJ’Muxe^^e^  (9) 

2=1 

N  N 

~2elFiri  xev~ 

2=1  2=1 
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Fig.  3.  Physical  meaning  of  the  triple  scalar  product.  For  agents  such  as  i 
on  the  near  side  of  the  midline,  defined  by  ev,  the  triple  scalar  product  is 
righthanded,  meaning  that  they  can  apply  forces  that  simultaneously  decrease 
both  error  signals.  While  on  the  far  side,  eu  and  ev  represent  conflicting 
goals. 


Note  that  the  first  term  is  zero  since  M  is  a  scalar  and 
ev  and  to  are  always  perpendicular.  The  second  and  fourth 
terms  are  clearly  non-positive;  and  only  zero  where  the  error 
terms  vanish.  The  third  term  is  a  triple  scalar  product  whose 
magnitude  is  bounded  by  |!ew||||ri||||e„||.  However,  its  sign 
cannot  be  determined  (see  Fig.  3  for  a  graphical  intuition). 
Fortunately  the  last  three  terms  can  be  collectively  bounded 
by 

N 

W<-^(\\ev\\-\\n\\\\eJ)2.  (10) 

2=1 


Time  (sec) 

Fig.  4.  Simulation  of  the  constant  velocity  controller.  Actual  global  frame 
velocities  are  dashed  lines.  Desired  global  frame  velocities  are  solid:  vx{m/s) 
(top,  green),  vy(m/s )  (middle,  blue),  and  ui  (rad/s)  (bottom,  red). 

IV.  Time  Varying  Velocity  Controller 

It  is  more  difficult  to  track  time  varying  velocities,  possibly 
in  the  presence  of  drag  forces,  in  a  distributed  fashion  since 
it  generally  requires  using  feed  forward  terms.  Allocating  the 
effects  of  those  terms  among  the  swarm  members  requires 
some  coordination. 

Assume  ifj  and  u>d  are  not  constant  in  the  global  frame; 
and  that  iff  and  6j,i  are  also  specified.  Now  the  derivatives 
contain  additional  terms 


The  only  scenario  in  which  W  =  0  would  be  if  all  the 
following  conditions  held  true  for  all  i  =  1,...,  N:  (1) 
Vi  is  perpendicular  to  e„;  (2)  efri  x  e„  <  0;  and  (3) 
||e„||  —  1 1 r-i 1 1 1 1 1 1  =  0.  Only  a  single  point  on  a  rigid  body 
satisfies  all  of  these  conditions.  Since  we  assume  N  >  1  and 
fi  7^  r2,  then  W  <  0. 

Remark  3.2:  While  the  form  of  (6)  is  convenient  for  the 
proof,  in  practice  it  is  much  easier  to  measure  the  agent’s 
velocity  rather  than  the  object’s.  (6)  can  be  rewritten  as 


ev  =  u:  x  RT  +  RtWv% 

N 

-fCd  XV  Fdrag  ~  M~X  ^  Fi 


N 


—  T~drag  J  ^  ^  G;  X  F{ . 


i=l 


Define  a  new  control  law 

Fi  =  F  +  Ff. 


(12) 


(13) 


(14) 


Fi  =  vd  +  tod  x  fi  -  Vi.  (11) 

Remark  3.3:  A  friction-based  contact-model  can  easily  be 
accommodated  in  this  scenario,  assuming  the  robots  are  in 
a  force  closure  configuration.  Each  robot  simply  computes  a 
desired  force  from  (6),  and  checks  to  see  if  it  is  inside  the 
set  of  admissible  contact  forces  defined  by  the  friction  cone, 
Fi.  If  Fi  <E  Fi,  Fi  v-  Fi,  else  Fi  <-  [0,0]T.  The  stability 
proof  is  not  qualitatively  altered  in  anyway;  however  the  rate 
of  convergence  may  be  slowed  since  at  any  given  time  there 
are  effectively  less  than  N  agents  who  are  applying  non-zero 
forces  in  (10). 

Figure  4  shows  a  simulation  of  the  control  law,  with  non¬ 
zero  initial  velocity.  The  object  is  a  rectangle,  2 (to)  x  1  (m), 
with  M  =  1  (kg).  15  agents  are  distributed  randomly  around 
the  perimeter  employing  the  control  law  (6).  The  desired 
velocities  are  vx  =  2 (m/s),  vy  =  1  (m/s),  and  u:  =  0 (rad/s). 
The  velocities  quickly  converge  to  the  desired  values. 


Using  the  Lyapunov  Function  defined  previously,  and  our 
result  from  the  constant  velocity  case,  the  derivative  now 
contains  two  new  terms: 

N 

w  <  -;£(!|ej-||ri||||ej)2+  (15) 

2=1 


N 


eTv  |  MRt{^  -  Fdrag + 
Tdrag  ~  ^  Ti  X  f  • 


2=1 


Suggesting  each  agent  should  compute  Ff  such  that 


labeleq  :  feedforward 


y.» 

w 

i _ 

.  £f=i  riXF? 

MRT{ij))v<f  —  t 

d  ‘Fd,  Tdrag 


(16) 


drag 


which  can  be  re-written  in  matrix  form. 

Problem  4.1:  Given  the  desired  net  feed  forward  wrench 


Fd  G 


compute  Fd  G  R2Ar  such  that 
BFd  =  pd 

where  B  =  [B\ . . .  Bi . . .  Bn\,  and 


(17) 


II,  = 


_ p  O  p  -L- 

i  i  _ 

Clearly,  under  the  assumption  that  there  are  two  or  more 
agents  that  are  not  collocated,  B  is  full  rank  and  a  solution 
exists.  In  fact, 

Fd  =  B^Fd,  (18) 

is  a  minimum  effort  solution,  where  /i  '  is  the  pseudo-inverse 
B'  (BB')-1 .  This  solution  was  used  in  [1]  and  is  analogous 
to  a  redundant  manipulator  controller. 

The  issue  we  consider  here  is:  is  it  possible  to  implement 
this  solution  in  an  entirely  distributed  fashion;  or,  if  not,  what 
information  much  be  shared  among  the  agents  to  implement 
the  solution.  Let  B  =  (BB1)  G  R3x3.  Thenagent  i’s  compo¬ 
nent  of  (18)  is 


Fig.  5.  Simulation  of  the  time  varying  desired  velocity  controller.  The  plot 
depicts  the:  x  component  of  ev  (m/s)  (middle,  green),  y  component  of  ev 
(bottom,  blue),  and  eu  (rad/s)  (top,  red). 


where 


Fd  = 


B  = 


0  -rf 
1  rf 


(19) 
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Therefore  each  agent  maintain  an  estimate  of  B,  called  II,, 
based  on  the  following  quantities. 

•  Fd  the  feed-forward  wrench  computed  from  (??). 

•  rf  and  rf:  its  own  location  in  the  body  frame  (assumed 
known). 

•  N:  the  number  of  agents  in  contact  with  the  object 
(assumed  known). 

•  my  =  XfHi  rf  and  mx  =  The  first  moments 

of  the  swarm’s  configuration  (estimated). 

•  mzz  =  y]^=1(rf)2  +  (rf)2:  The  second  moment  of  the 
swarms’s  configuration  about  the  rotational  axis  (esti¬ 
mated). 

In  order  to  estimate  the  values  of  the  moments  in  a  dis¬ 
tributed  fashion,  we  employ  the  consensus  protocol  discussed 
in  [9],  Let  rhi(t)  =  [mf,mf,mf2]T  be  the  ith  agent’s 
estimate  of  the  appropriate  moment  at  time  t.  Estimates  are 
updated  according  to  the  following  dynamics 
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Fig.  6.  Each  agents  estimate  of  the  swarm’s  moments.  mx  (top,  green),  my 
(middle,  blue),  and  mzz  (bottom,  red). 


Since  the  dynamics  of  estimates  are  decoupled  from  the 
dynamics  of  the  object,  to  prove  the  decentralized  controller’s 
stability  it  suffices  to  show  that  the  feed  forward  term  is  always 
bounded.  That  is  equivalent  to  ensuring  the  determinant  of  B 


Nrhzz  -  (mf)2  -  (mf)2  /  0. 


(22) 


rhi  =  ^2  (rhj  -  mf)  (21) 

jeMi 

using  initial  condition  TOi(0)  =  [Nrf ,  Nrf ,  N(rlx)2+N(rly)2]. 

In  [11]  it  was  shown  that,  provided  the  underlying  graph  is 
connected,  such  protocols  are  globally  exponentially  stable; 
and  that  the  equilibrium  value  is  a  consensus  equal  to  the 
mean  of  the  agents’s  initial  conditions.  Therefore  m,(f)  — > 
[■ mx ,  my,  mzz ]  as  t  — >  oo. 


The  only  situations  in  which  this  happens  are  when  all  the 
agents  are  co-located  (or  there  is  only  a  single  agent).  Our  as¬ 
sumption  is  that  the  agents  are  in  a  force  closure  configuration, 
which  precludes  this  possibility.  Therefore  the  equilibrium 
(consensus)  value  never  causes  the  determinant  to  be  zero. 
However  the  initial  condition  does  violate  this  condition.  One 
possible  approach,  used  in  the  simulation  below,  is  to  not 
activate  the  pushing  control  laws  until  after  the  consensus 
protocol  has  gone  through  at  least  one  iteration. 


Fig.  7.  The  determinant  of  each  agent’s  B  matrix.  Note  they  are  non-zero 
after  the  first  iteration. 


Fig.  8.  A  screen  shot  of  the  simulation.  Green  lines  represent  the  agents 
forces.  The  path  of  the  object’s  center  of  mass  is  a  circle. 


Figure  5  shows  a  simulation  of  the  time  varying  velocity 
control  law,  with  non-zero  initial  velocity.  The  velocity  track¬ 
ing  errors  quickly  converge  to  zero.  The  object  was  a  rectangle 
2 (to)  x  1  (to)  with  M  =  1  (kg).  15  agents  are  distributed 
randomly  around  the  perimeter  employing  the  control  law 
(14)  and  (19).  The  desired  velocities  are  vx  =  t(m/s), 
vy  =  t(m/s),  and  to  =  0.\t(rad/ s).  The  communication 
network  graph  is  a  ring  (i.e.  the  set  of  edges  E  contains  only 
ei(i+i)  for  all  z  <  N-l  and  euv).  Figure  6  show  each  agent’s 
estimate  of  the  swarm’s  configuration  (moments  mx(t ),  rhv(t ), 
rhzz(t));  while  Figure  7  shows  that  the  determinant  of  BB' 
is  non-zero  for  all  but  the  initial  estimates,  implying  that  the 
distributed  system  converges  to  the  desired  velocity.  Figure  8 
shows  another  example  scenario.  Here,  the  desired  velocity 


causes  the  object  to  move  in  a  circular  pattern.  The  figure 
illustrates  the  path  of  the  object’s  center  of  mass,  and  shows 
the  force  vector  of  each  agent. 

Remark  4.2:  It  is  more  difficult  to  accommodate  a  friction- 
based  contact-model  in  this  scenario  because,  instead  of  simply 
depending  on  N  (a  constant),  the  controller  must  estimate 
the  number  of  agents  who  are  capable  of  pushing  the  object, 
subject  to  friction  constraints,  at  any  given  instant  in  time. 
This  is  a  topic  of  ongoing  work. 

V.  Discussion  of  Information  Requirements 

Regarding  the  constant  velocity  control  law  in  Sect.  Ill,  each 
agent  needs  to  know: 

•  the  desired  velocities  Vd  and  jJ,i, 

•  the  actual  velocity  of  the  object  v  and  to  (alternatively, 
its  own  velocity  Vi); 

•  and  its  own  location  relative  to  the  center  of  mass  r  j . 
No  knowledge  of  M,  J,  the  number  of  agents  N,  or  the 
other  agent’s  actions  are  required  for  stable  velocity  control. 
Regarding  the  rate  of  convergence,  note  from  (10)  that  adding 
more  agents  never  decreases  the  convergence  rate  -  and 
generally  improves  it. 

The  conservative  bound  in  (10)  stems  from  the  case  depicted 
in  Figure  3.  In  this  case  the  error  signals  ev  and  ew  are 
essentially  parsimonious  requirements  for  agent  i\  however, 
for  agents  on  the  far  side  of  the  midline  ev  and  eu  represent 
competing  requirements. 

Regarding  the  time  varying  velocity  control  law  in  Sect.  IV, 
in  addition  to  the  information  requirements  for  the  constant 
velocity  control  law,  each  agent  needs  knowledge  of: 

•  the  object’s  mass  M  and  inertia  J; 

•  the  desired  accelerations  Vd  and  u>d\  and 

•  the  number  of  agents  in  contact  with  the  object  N. 

Finally  the  rate  of  convergence  of  the  estimates  m  is  related 

to  the  second  smallest  Eigenvalue  of  the  graph  LaPlacian, 
A2  (/,(;)  >  also  known  as  the  algebraic  connectivity  of  the 
graph  -  a  measure  of  how  strongly  connected  the  graph 
is.  The  ring  topology  used  in  the  example  has  rather  weak 
connectivity  (small  A 2  ( /<; ) )  while  a  complete  graph  has 
very  strong  connectivity.  In  general  adding  links  increases 
A2 {Lq)  improving  the  convergence  rate  of  to.  While  we  only 
consider  networks  with  static  topology  and  no  network  delays, 
analogous  protocols  have  been  defined  for  networks  where 
those  assumptions  are  relaxed  [11].  They  have  been  shown  to 
have  similar  convergence  properties  and  could  be  applied  to 
this  problem. 


VI.  Conclusion 

In  this  paper  we  consider  cooperative  manipulation  prob¬ 
lems  where  a  large  group  (swarm)  of  non-articulated  mobile 
robots  is  trying  to  cooperatively  control  the  velocity  of  some 
larger  rigid  body  by  exerting  forces  around  its  perimeter.  We 
consider  a  second  order  dynamic  model  for  the  object  but  use  a 
simplified  contact  model.  We  present  two  asymptotically  stable 
control  laws.  In  the  case  of  a  constant  desired  velocity  and  no 


drag,  it  is  shown  that  no  coordination  is  required  between  the 
swarm  members  and  no  knowledge  of  the  dynamic  parameters 
of  the  object  is  needed.  For  more  complex  trajectories,  and 
drag  forces,  we  introduce  a  decentralized  feed-forward  compo¬ 
nent  that  requires  some  knowledge  of  the  object’s  parameters 
and  the  swarm’s  geometric  configuration.  An  online  distributed 
consensus  protocol  is  used  to  estimate  swarm’s  configuration. 
An  area  of  future  work  is  to  employ  a  distributed  method  to 
estimate  the  number  of  agents  in  contact  with  the  object,  N, 
online  -  a  census  algorithm  [6], 
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